#include "GPSFraming.h"

#define BUFF_SIZE 512

int GetNextMsg(FILE *in, uint8_t *buffer, size_t buffsize)
{
	uint16_t message_size = 0;
	uint16_t extra_len=0;
	
	if( fread(&message_size,sizeof(uint16_t),1,in) != 1)
		return -1;
	message_size = ntohs(message_size);
	
	if(message_size > buffsize)
	{
		extra_len = message_size-buffsize;
		message_size = buffsize;
	}
	
	if( fread(buffer, sizeof(uint8_t), message_size, in) != message_size)
	{
		fprintf(stderr, "Did not recieve expected packet");
		return -1;
	}
	
	if(extra_len > 0)
	{
		uint8_t waste[BUFF_SIZE];
		fread(waste,sizeof(uint8_t), extra_len, in);
		return -(message_size + extra_len);
	}
	
	return message_size;
}

int PutMsg(void *message, size_t size, FILE *out)
{
	uint16_t length = htons(size);
	if( (fwrite(&length, sizeof(uint16_t), 1, out) != 1) || (fwrite(message, sizeof(uint8_t), size, out) != size))
		return -1;
	
	fflush(out);
	return size;
}

void ExitWithMessage(char* message)
{
	printf("%s\n", message);
	exit(1);
}
